/* 3D Navier-Stokes Solver */

#include "main.h"
#include "typen.h"
#include "matrix.h"
#include "navier.h"
#include <math.h>
#include <stdlib.h>

#define DDP(L,M,R,DA,i) (S.ddPstar[DA][0][i]*L+S.ddPstar[DA][1][i]*M+S.ddPstar[DA][2][i]*R)

void Navier::SetPLocalBorder(Matrix <double> &X, int i, int j, int k)
{
    /*IFAIR(flag[i-1][j][k]) X[i-1][j][k]=X[i][j][k];
    IFAIR(flag[i+1][j][k]) X[i+1][j][k]=X[i][j][k];
    IFAIR(flag[i][j-1][k]) X[i][j-1][k]=X[i][j][k];
    IFAIR(flag[i][j+1][k]) X[i][j+1][k]=X[i][j][k];
    IFAIR(flag[i][j][k-1]) X[i][j][k-1]=X[i][j][k];
    IFAIR(flag[i][j][k+1]) X[i][j][k+1]=X[i][j][k];*/
   if(i==0)       X[0][j][k] = X[1][j][k];   
   if(i==NX+1)      X[NX+1][j][k] = X[NX][j][k];         
   if(j==0)       X[i][0][k] = X[i][1][k];
   if(j==NY+1)      X[i][NY+1][k] = X[i][NY][k];
   if(k==0)       X[i][j][0] = X[i][j][1];
   if(k==NZ+1)      X[i][j][NZ+1] = X[i][j][NZ];   
}

double  Navier::CompPoisson(/*double starttime*/)
{ 
   return Poisson(); // Solve Pressure equation  
}
 
double Navier::CalcRes() 
{
   double res=0.0, sq;
   int count=0;
   int i,j,k;
   IJKALLLOOP 
      IFFLUID_OUTFLOW(flag[i][j][k])
      {                                                
         //SetPLocalBorder(P,i,j,k);
         /*sq = DDP(P[i-1][j][k],P[i][j][k],P[i+1][j][k],0,i) +                            
            DDP(P[i][j-1][k],P[i][j][k],P[i][j+1][k],1,j) +                            
            DDP(P[i][j][k-1],P[i][j][k],P[i][j][k+1],2,k) - RHS[i][j][k];*/
         sq = MatVecMult(P,i,j,k,1,ISFLUID(flag[i][j][k]));
         res += sq*sq;
         count++;
      }
   if(count==0) return 0.0;
   return (sqrt(res/count)/*S.delt*/);
}

// solve linear system "\Delta p=RHS" using SOR iteration in a 8-color scheme
// boundary values are set before each SOR iteration 
// iteration is terminated when 
//   - the residue is too small 
//   - a maximum value of iterations is exceeded
// if SSOR is defined, symmetric SOR iteration is used. 
//

double Navier::Poisson() 
{
   double    res;
   // Use BICGStab-Solver
   if (S.Solver == 5 || S.Solver == 6)
      res=BiCGStab();
   return (res);
}

void  Navier::CalcKoeffPreCond()
{
    int i,j,k;
    
    // Jacobi Preconditioner coeff
    IJKALLLOOP //
      IFFLUID_OUTFLOW(flag[i][j][k])
      {
         KoeffPreCond[i][j][k]= -1.0/(S.ddPstar[0][1][i]+S.ddPstar[1][1][j]+S.ddPstar[2][1][k]);
      }
}


inline double Navier::Precondition(double P, int i, int j, int k){
  return KoeffPreCond[i][j][k] * P;
}

inline double Navier::PreconditionBack(double P, int i, int j, int k)
{
  return 1.0/KoeffPreCond[i][j][k] * P;
}

inline double  Navier::MatVecMult(Matrix<double>& A,int i, int j, int k, int RES, bool flag_bool)
{
   double result;   
   double PMIN = 0.0;
   /*
      result= S.ddPstar[0][2][i]*A[i+1][j][k] + S.ddPstar[0][0][i]*A[i-1][j][k] + 
      S.ddPstar[1][2][j]*A[i][j+1][k] + S.ddPstar[1][0][j]*A[i][j-1][k] +
      S.ddPstar[2][2][k]*A[i][j][k+1] + S.ddPstar[2][0][k]*A[i][j][k-1] +
      (S.ddPstar[0][1][i]+S.ddPstar[1][1][j]+S.ddPstar[2][1][k]) * A[i][j][k]; 
   */
   double B[6];
   if(flag_bool)
   {
      if(ISAIR(flag[i+1][j][k]))
      {
         B[0] = PMIN;
      }
      else
      {
         if(ISGIVEN(flag[i+1][j][k]))
            B[0] = A[i][j][k];
         else
            B[0] = A[i+1][j][k];
      }
      if(ISAIR(flag[i-1][j][k]))
      {
         B[1] = PMIN;
      }
      else
      {
         if(ISGIVEN(flag[i-1][j][k]))
            B[1] = A[i][j][k];
         else
            B[1] = A[i-1][j][k];
      }
      if(ISAIR(flag[i][j+1][k]))
      {
         B[2] = PMIN;
      }
      else
      {
         if(ISGIVEN(flag[i][j+1][k]))
            B[2] = A[i][j][k];
         else
            B[2] = A[i][j+1][k];
      }
      if(ISAIR(flag[i][j-1][k]))
      {
         B[3] = PMIN;
      }
      else
      {
         if(ISGIVEN(flag[i][j-1][k]))
            B[3] = A[i][j][k];
         else
            B[3] = A[i][j-1][k];
      }
      if(ISAIR(flag[i][j][k+1]))
      {
         B[4] = PMIN;
      }
      else 
      {
         if(ISGIVEN(flag[i][j][k+1]))
            B[4] = A[i][j][k];
         else
            B[4] = A[i][j][k+1];
      }
      if(ISAIR(flag[i][j][k-1]))
      {
         B[5] = PMIN;
      }
      else 
      {
         if(ISGIVEN(flag[i][j][k-1]))
            B[5] = A[i][j][k];
         else 
            B[5] = A[i][j][k-1];
      }
      result = S.ddPstar[0][2][i]*B[0] + S.ddPstar[0][0][i]*B[1] + 
     S.ddPstar[1][2][j]*B[2] + S.ddPstar[1][0][j]*B[3] +
     S.ddPstar[2][2][k]*B[4] + S.ddPstar[2][0][k]*B[5] +
     (S.ddPstar[0][1][i]+S.ddPstar[1][1][j]+S.ddPstar[2][1][k]) * A[i][j][k];
   }
   else
   {
      result = 0;
      double den = 0;
      if(i<=S.gridp[0])
      {
         if(ISAIR(flag[i+1][j][k]))
         {
            B[0] = PMIN;
         }
         else
         {
            if(ISGIVEN(flag[i+1][j][k]))
               B[0] = A[i][j][k];
            else
               B[0] = A[i+1][j][k];
         }
         result += S.ddPstar[0][2][i]*B[0];
         den += S.ddPstar[0][2][i];
      }
      if(i>0)
      {
         if(ISAIR(flag[i-1][j][k]))
         {
            B[1] = PMIN;
         }
         else
         {
            if(ISGIVEN(flag[i-1][j][k]))
               B[1] = A[i][j][k];
            else
               B[1] = A[i-1][j][k];
         }
         result += S.ddPstar[0][0][i]*B[1];
         den += S.ddPstar[0][0][i];
      }
      if(j<=S.gridp[1])
      {
         if(ISAIR(flag[i][j+1][k]))
         {
            B[2] = PMIN;
         }
         else
         {
            if(ISGIVEN(flag[i][j+1][k]))
               B[2] = A[i][j][k];
            else
               B[2] = A[i][j+1][k];
         }
         result += S.ddPstar[1][2][j]*B[2];
         den += S.ddPstar[1][2][j];
      }
      if(j>0)
      {
         if(ISAIR(flag[i][j-1][k]))
         {
            B[3] = PMIN;
         }
         else
         {
            if(ISGIVEN(flag[i][j-1][k]))
               B[3] = A[i][j][k];
            else
               B[3] = A[i][j-1][k];
         }
         result += S.ddPstar[1][0][j]*B[3];
         den += S.ddPstar[1][0][j];
      }
      if(k<=S.gridp[2])
      {
         if(ISAIR(flag[i][j][k+1]))
         {
            B[4] = PMIN;
         }
         else 
         {
            if(ISGIVEN(flag[i][j][k+1]))
               B[4] = A[i][j][k];
            else
               B[4] = A[i][j][k+1];
         }
         result += S.ddPstar[2][2][k]*B[4];
         den += S.ddPstar[2][2][k];
      }
      if(k>0)
      {
         if(ISAIR(flag[i][j][k-1]))
         {
            B[5] = PMIN;
         }
         else 
         {
            if(ISGIVEN(flag[i][j][k-1]))
               B[5] = A[i][j][k];
            else 
               B[5] = A[i][j][k-1];
         }
         result += S.ddPstar[2][0][k]*B[5];
         den += S.ddPstar[2][0][k];
      }
      result -=  den * A[i][j][k];
   }
   
   //if(k==15 && j==2)
     // cout<< "\n i =  " << i << "RESULT Antes= " << result; 
   if(RES) 
      result=RHS[i][j][k]-result;
   //if(k==15 && j==2)
     // cout<< "\t RHS = " << RHS[i][j][k];

  return result;
}


double Navier::BiCGStab() 
{
   double a,b,w1,w2,w,res=1E9,norm_vj,norm_r0,norm_sj,norm_rj ;
   double rj1r0, rjr0, sig ;
   int i,j,k;
   int it=0;
   //double ijkroot = 1./(S.gridp[0]*S.gridp[1]*S.gridp[2]-S.ObstacleCount);
   //double pmin = 0.0;
   CalcKoeffPreCond();

   restart:
   rjr0=norm_r0=0.0 ;
   IJKALLLOOP 
      IFFLUID_OUTFLOW(flag[i][j][k]) 
      {
         //SetPLocalBorder(P,i,j,k);
         BiCG_r0[i][j][k]=BiCG_pj[i][j][k]=BiCG_rj[i][j][k]=MatVecMult(P,i,j,k,1,ISFLUID(flag[i][j][k]));
         rjr0 += BiCG_rj[i][j][k]*BiCG_r0[i][j][k] ;
      }
         
   norm_r0=sqrt(rjr0);
   IJKALLLOOP 
      IFFLUID_OUTFLOW(flag[i][j][k]) 
         P[i][j][k] = PreconditionBack(P[i][j][k],i,j,k);  //  PRECONDITIONBACK P    
    
   if((res>=S.eps) && (it<S.itermax))
   {	
      do
      {
         sig=0.0;
         norm_vj=0.0;
         norm_rj=0.0;
  
         // APpj      
         IJKALLLOOP 
            IFFLUID_OUTFLOW(flag[i][j][k]) 
               MatBuf[i][j][k] = Precondition(BiCG_pj[i][j][k],i,j,k);  //  PRECONDITION pj 
	    
         IJKALLLOOP 
            IFFLUID_OUTFLOW(flag[i][j][k])
            {
               //SetPLocalBorder(MatBuf,i,j,k);
               BiCG_vj[i][j][k] = MatVecMult(MatBuf,i,j,k,0,ISFLUID(flag[i][j][k]));  	
               sig   += BiCG_vj[i][j][k] * BiCG_r0[i][j][k];
               norm_vj += BiCG_vj[i][j][k] * BiCG_vj[i][j][k];
               norm_rj += BiCG_rj[i][j][k] * BiCG_rj[i][j][k]; // NEU
            }
	    
         norm_vj = sqrt(norm_vj);         
         norm_rj = sqrt(norm_rj);  // NEU
         a=rjr0 / sig ;
	    
         if(fabs(sig) <= (1e-12*(norm_vj*norm_r0)))
         {
            IJKALLLOOP IFFLUID_OUTFLOW(flag[i][j][k]) 
            P[i][j][k]=Precondition(P[i][j][k],i,j,k);  //  PRECONDITION P		
            res=CalcRes(); it++;
		
            goto restart ;
         }
         if((fabs(a)*norm_vj/(norm_rj==0?1E-15:norm_rj))<=0.08)
         {
            IJKALLLOOP 
               IFFLUID_OUTFLOW(flag[i][j][k]) 
                  P[i][j][k]=Precondition(P[i][j][k],i,j,k);  //  PRECONDITION P
		
            res=CalcRes(); it++;
		
            goto restart;  // NEU
         }
	    
	    // sj =rj -a*vj
         IJKALLLOOP 
            IFFLUID_OUTFLOW(flag[i][j][k]) 
               BiCG_sj[i][j][k] = BiCG_rj[i][j][k]-a*BiCG_vj[i][j][k] ;
      
         norm_sj=0.0;
         IJKALLLOOP 
            IFFLUID_OUTFLOW(flag[i][j][k]) 
               norm_sj += BiCG_sj[i][j][k]*BiCG_sj[i][j][k];
	    
         norm_sj=sqrt(norm_sj);
	    
         if(norm_sj>S.eps)
         {
            // tj = Asj 
            IJKALLLOOP 
               IFFLUID_OUTFLOW(flag[i][j][k]) 
                  MatBuf[i][j][k] = Precondition(BiCG_sj[i][j][k],i,j,k);  //  PRECONDITION sj
		
            IJKALLLOOP 
               IFFLUID_OUTFLOW(flag[i][j][k])
               {
                  //SetPLocalBorder(MatBuf,i,j,k);		    
                  BiCG_tj[i][j][k]=MatVecMult(MatBuf,i,j,k,0,ISFLUID(flag[i][j][k])); 	
               }
            w1=w2=0.0;
         IJKALLLOOP 
            IFFLUID_OUTFLOW(flag[i][j][k])
            {
               w1 += BiCG_tj[i][j][k]*BiCG_sj[i][j][k];
               w2 += BiCG_tj[i][j][k]*BiCG_tj[i][j][k];
            }
            w=w1/(w2==0?1E-15:w2);
		
            IJKALLLOOP 
               IFFLUID_OUTFLOW(flag[i][j][k]) 
                  P[i][j][k] += a*BiCG_pj[i][j][k] + w*BiCG_sj[i][j][k];
		
            // rj+1 = sj -w*tj
            IJKALLLOOP 
               IFFLUID_OUTFLOW(flag[i][j][k]) 
                  BiCG_rj[i][j][k] = BiCG_sj[i][j][k]-w*BiCG_tj[i][j][k] ;
		
            // < rj+1, r0 >  
            rj1r0=0.0;
            IJKALLLOOP 
               IFFLUID_OUTFLOW(flag[i][j][k])
                  rj1r0 += BiCG_rj[i][j][k]*BiCG_r0[i][j][k] ;
		
            b=a*rj1r0/(w*rjr0==0?1E-15:(w*rjr0));
		
            IJKALLLOOP IFFLUID_OUTFLOW(flag[i][j][k]) 
               BiCG_pj[i][j][k] = BiCG_rj[i][j][k] + b*(BiCG_pj[i][j][k]-w*BiCG_vj[i][j][k]);
		
         }
         else
         {
            IJKALLLOOP 
               IFFLUID_OUTFLOW(flag[i][j][k]) 
                  P[i][j][k] += a*BiCG_pj[i][j][k];
		
            rj1r0=0.0 ;
            IJKALLLOOP 
               IFFLUID_OUTFLOW(flag[i][j][k]) 
               {
                  BiCG_rj[i][j][k] = BiCG_sj[i][j][k];
                  rj1r0 += BiCG_rj[i][j][k]*BiCG_r0[i][j][k] ;
               }
		
         }
	    
         // j=j+1 
         rjr0 = rj1r0 ;
	    
         res=0.0;
         int count = 0;
         IJKALLLOOP 
            IFFLUID_OUTFLOW(flag[i][j][k])
            {
               res += BiCG_rj[i][j][k]*BiCG_rj[i][j][k]; 
               count ++;
            }
         if(count == 0) res = 0.0;
         else res=sqrt(res/count) /*S.delt*/;
	    
         it++;
	    	    
      }while( (res>=S.eps) && (it<S.itermax) ); 
	
   } // End If
    
    
   IJKALLLOOP 
      IFFLUID_OUTFLOW(flag[i][j][k]) 
         P[i][j][k]=Precondition(P[i][j][k],i,j,k);    //  PRECONDITION P
      else if(P[i][j][k] != 0.0)
         std::cout<<"***********("<<i<<","<<j<<","<<k<<"),"<< P[i][j][k]<<"\n";
   res=CalcRes(); 

   iterations = it ;   
   alliterations+=iterations;
    
   return res;
}


